﻿using System;
using System.IO;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Microsoft.Xna.Framework;

namespace Drifting.Foundation
{
    public delegate double DistanceFunction(double[] x1, double[] x2);

    public delegate bool GoalCheck(double[] q_test);

    static public class MathAssistant
    {
        public const double PI_half = Math.PI * 0.5;
        public const double PI_double = Math.PI + Math.PI;
        public const float PI_double_float = (float)PI_double;

        static public double Euclidean(double[] f1, double[] f2)
        {
            double sum = 0;
            for (int i = 0; i < f1.Length; i++)
            {
                double di = f2[i] - f1[i];
                sum += di * di;
            }
            return Math.Sqrt(sum);
        }

        static public double Euclidean(float[] f1, float[] f2)
        {
            double sum = 0;
            for (int i = 0; i < f1.Length; i++)
            {
                double di = f2[i] - f1[i];
                sum += di * di;
            }
            return Math.Sqrt(sum);
        }

        static public double AngleFromSinCos(double sin, double cos)
        {
            return Math.Atan2(sin, cos);
        }

        static public float AngleFromSinCos(float sin, float cos)
        {
            return (float)Math.Atan2(sin, cos);
        }

        static public double Norm(double[] x1)
        {
            double sum = 0;
            for (int i = 0; i < x1.Length; ++i)
            {
                double dxi = x1[i];
                sum += dxi * dxi;
            }

            sum = Math.Sqrt(sum);

            return sum;
        }

        static public void vector_clear(ref double[] v)
        {
            int n = v.Length;
            for (int i = 0; i < n; ++i)
                v[i] = 0;
        }

        static public void vector_normalize3(ref double[] v)
        {
            double k = Math.Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
            if (k == 0) throw new ArgumentException("Zero vector");
            k = 1 / k;
            v[0] *= k;
            v[1] *= k;
            v[2] *= k;
        }

        static public void matrix_set(double[,] m_src, ref double[,] m_dst)
        {
            int m = m_src.GetLength(0);
            int n = m_src.GetLength(1);

            for (int i = 0; i < m; ++i)
                for (int j = 0; j < n; ++j)
                    m_dst[i, j] = m_src[i, j];
        }

        static public void matrix_validation(ref double[,] matrix, double MATRIX_ZERO)
        {
            int m = matrix.GetLength(0);
            int n = matrix.GetLength(1);

            for (int i = 0; i < m; ++i)
                for (int j = 0; j < n; ++j)
                {
                    if (Math.Abs(matrix[i, j]) < MATRIX_ZERO)
                        matrix[i, j] = MATRIX_ZERO * Math.Sign(matrix[i, j]);
                }
        }

        static public void vector_set(double[] q_src, ref double[] q_dst)
        {
            if (q_dst == null)
                q_dst = new double[q_src.Length];
            for (int i = 0; i < q_src.Length; ++i)
                q_dst[i] = q_src[i];
        }

        static public void vector_set(double[] q_src, ref double[] q_dst, int idx)
        {
            if (q_dst == null)
                q_dst = new double[q_src.Length + idx];
            for (int i = 0; i < q_src.Length; ++i)
                q_dst[i + idx] = q_src[i];
        }

        static public void vector_add(double[] q_1, double[] q_2, ref double[] q_out)
        {
            if (q_1 == null)
                throw new NullReferenceException();
            if (q_2 == null)
                throw new NullReferenceException();
            if (q_1.Length != q_2.Length)
                throw new Exception("vector dimension mismatch.");
            if (q_out == null)
                q_out = new double[q_1.Length];
            for (int i = 0; i < q_1.Length; ++i)
                q_out[i] = q_1[i] + q_2[i];
        }

        static public void vector_kTimes(double[] v, double k, ref double[] v_out)
        {
            for (int i = 0; i < v.Length; ++i)
                v_out[i] = v[i] * k;
        }

        static public void vector_vlinear(double[] v, double k, double[] b, ref double[] v_out)
        {
            for (int i = 0; i < v.Length; ++i)
                v_out[i] = v[i] * k + b[i];
        }

        static public void QuaternionFromAxisAngle(double e0, double e1, double e2, double angle, ref double q0, ref double q1, ref double q2, ref double w)
        {
            double length = Math.Sqrt(e0 * e0 + e1 * e1 + e2 * e2);
            double sinhA = Math.Sin(angle / 2);
            if (length > 0.000000001)
            {
                e0 /= length;
                e1 /= length;
                e2 /= length;
            }
            else
            {
                e0 = 0;
                e1 = 1;
                e2 = 0;
            }

            q0 = e0 * sinhA;
            q1 = e1 * sinhA;
            q2 = e2 * sinhA;
            w = Math.Cos(angle / 2);
        }

        static public void AxisAngleFromQuaternion(double q0, double q1, double q2, double w, ref double e0, ref double e1, ref double e2, ref double angle)
        {
            angle = 2 * Math.Acos(w);
            double length = Math.Sqrt(q0 * q0 + q1 * q1 + q2 * q2);

            e0 = q0 / length;
            e1 = q1 / length;
            e2 = q2 / length;
        }

        static public void QuaternionNormalize(ref double q0, ref double q1, ref double q2, ref double qw)
        {
            double magnitude = Math.Sqrt(q0 * q0 + q1 * q1 + q2 * q2 + qw * qw);

            if (magnitude < 0.0000000001)
                throw new ArgumentException();

            q0 /= magnitude;
            q1 /= magnitude;
            q2 /= magnitude;
            qw /= magnitude;
        }

        static public void QuaternionProduct(
            double w0, double x0, double y0, double z0,
            double w1, double x1, double y1, double z1,
            ref double w2, ref double x2, ref double y2, ref double z2
            )
        {
            w2 = w0 * w1 - x0 * x1 - y0 * y1 - z0 * z1;
            x2 = x0 * w1 + w0 * x1 - z0 * y1 + y0 * z1;
            y2 = y0 * w1 + z0 * x1 + w0 * y1 - x0 * z1;
            z2 = z0 * w1 - y0 * x1 + x0 * y1 + w0 * z1;
        }
        
        static public bool CreateMatrixFromRotationVector_BoxToWorld(double Ax, double Ay, double Az, ref double[,] matrix)
        {
            if (matrix == null)
                matrix = new double[3, 3];
            double Angle = Math.Sqrt(Ax * Ax + Az * Az + Ay * Ay);
            if (Angle == 0)
            {
                matrix[0, 0] = 1;
                matrix[0, 1] = 0;
                matrix[0, 2] = 0;

                matrix[1, 0] = 0;
                matrix[1, 1] = 1;
                matrix[1, 2] = 0;

                matrix[2, 0] = 0;
                matrix[2, 1] = 0;
                matrix[2, 2] = 1;
                return false;
            }

            double AaxisX = Ax / Angle;
            double AaxisY = Ay / Angle;
            double AaxisZ = Az / Angle;
            double c = Math.Cos(Angle);
            double s = Math.Sin(Angle);
            double t = 1 - c;

            matrix[0, 0] = c + AaxisX * AaxisX * t;
            matrix[1, 1] = c + AaxisY * AaxisY * t;
            matrix[2, 2] = c + AaxisZ * AaxisZ * t;

            double tmp1 = AaxisX * AaxisY * t;
            double tmp2 = AaxisZ * s;
            matrix[1, 0] = tmp1 + tmp2;
            matrix[0, 1] = tmp1 - tmp2;
            tmp1 = AaxisX * AaxisZ * t;
            tmp2 = AaxisY * s;
            matrix[2, 0] = tmp1 - tmp2;
            matrix[0, 2] = tmp1 + tmp2;
            tmp1 = AaxisY * AaxisZ * t;
            tmp2 = AaxisX * s;
            matrix[2, 1] = tmp1 + tmp2;
            matrix[1, 2] = tmp1 - tmp2;

            return true;
        }

        static public void CreateMatrixFromQuaternion(
            double q1, double q2, double q3, double q4,
            ref double M00, ref double M01, ref double M02,
            ref double M10, ref double M11, ref double M12,
            ref double M20, ref double M21, ref double M22)
        {
            M00 = 1 - 2 * q2 * q2 - 2 * q3 * q3;
            M01 = 2 * (q1 * q2 - q3 * q4);
            M02 = 2 * (q1 * q3 + q2 * q4);

            M10 = 2 * (q1 * q2 + q3 * q4);
            M11 = 1 - 2 * q1 * q1 - 2 * q3 * q3;
            M12 = 2 * (q2 * q3 - q1 * q4);

            M20 = 2 * (q1 * q3 - q2 * q4);
            M21 = 2 * (q1 * q4 + q2 * q3);
            M22 = 1 - 2 * q1 * q1 - 2 * q2 * q2;
        }

        static public bool CreateMatrixFromRotationVector_BoxToWorld(
            double Ax, double Ay, double Az, 
            ref double M00, ref double M01, ref double M02,
            ref double M10, ref double M11, ref double M12,
            ref double M20, ref double M21, ref double M22)
        {
            double Angle = Math.Sqrt(Ax * Ax + Az * Az + Ay * Ay);
            if (Angle == 0)
            {
                M00 = 1;
                M01 = 0;
                M02 = 0;

                M10 = 0;
                M11 = 1;
                M12 = 0;

                M20 = 0;
                M21 = 0;
                M22 = 1;
                return false;
            }

            double AaxisX = Ax / Angle;
            double AaxisY = Ay / Angle;
            double AaxisZ = Az / Angle;
            double c = Math.Cos(Angle);
            double s = Math.Sin(Angle);
            double t = 1 - c;

            M00 = c + AaxisX * AaxisX * t;
            M11 = c + AaxisY * AaxisY * t;
            M22 = c + AaxisZ * AaxisZ * t;

            double tmp1 = AaxisX * AaxisY * t;
            double tmp2 = AaxisZ * s;
            M10 = tmp1 + tmp2;
            M01 = tmp1 - tmp2;
            tmp1 = AaxisX * AaxisZ * t;
            tmp2 = AaxisY * s;
            M20 = tmp1 - tmp2;
            M02 = tmp1 + tmp2;
            tmp1 = AaxisY * AaxisZ * t;
            tmp2 = AaxisX * s;
            M21 = tmp1 + tmp2;
            M12 = tmp1 - tmp2;

            return true;
        }

        static public void Matrix_X_(double[,] A, double[,] B, ref double[,] dst)
        {
            int m = A.GetLength(0);
            int n = B.GetLength(1);
            for (int i = 0; i < m; ++i)
                for (int j = 0; j < n; ++j)
                {
                    int l = A.GetLength(1);
                    dst[i, j] = 0;
                    for (int k = 0; k < l; ++k)
                        dst[i, j] += A[i, k] * B[k, j];
                }
        }

        static public void vector_transform(double[,] M, double[] src, int offset, ref double[] y)
        {
            int m = M.GetLength(0);
            int n = M.GetLength(1);
            for (int i = 0; i < m; ++i)
            {
                y[i] = 0;
                for (int j = 0; j < n; ++j)
                    y[i] += M[i, j] * src[offset + j];
            }
        }

        static public void vector_transform(double[,] M, double[] x, ref double[] y)
        {
            int m = M.GetLength(0);
            int n = M.GetLength(1);
            for (int i = 0; i < m; ++i)
            {
                y[i] = 0;
                for (int j = 0; j < n; ++j)
                    y[i] += M[i, j] * x[j];
            }
        }

        static public void vector_transform(double[,] M, double x0, double x1, double x2, ref double y0, ref double y1, ref double y2)
        {
            y0 = M[0, 0] * x0 + M[0, 1] * x1 + M[0, 2] * x2;
            y1 = M[1, 0] * x0 + M[1, 1] * x1 + M[1, 2] * x2;
            y2 = M[2, 0] * x0 + M[2, 1] * x1 + M[2, 2] * x2;
        }

        static public void vector_transform(double[,] M, double x0, double x1, double x2, ref double[] dst, int idx)
        {
            dst[idx + 0] = M[0, 0] * x0 + M[0, 1] * x1 + M[0, 2] * x2;
            dst[idx + 1] = M[1, 0] * x0 + M[1, 1] * x1 + M[1, 2] * x2;
            dst[idx + 2] = M[2, 0] * x0 + M[2, 1] * x1 + M[2, 2] * x2;
        }

        static public double vector_dotProduct(double[] u, double[] v)
        {
            return u[0] * v[0] + u[1] * v[1] + u[2] * v[2];
        }

        static public double vector_dotProduct(Vector3 u, Vector3 v)
        {
            return u.X * v.X + u.Y * v.Y + u.Z * v.Z;
        }

        static public void vector_cross3(double[] u, double[] v, ref double[] s)
        {
            s[0] = u[1] * v[2] - u[2] * v[1];
            s[1] = u[2] * v[0] - u[0] * v[2];
            s[2] = u[0] * v[1] - u[1] * v[0];
        }

        static public Vector3 vector_cross3(Vector3 u, Vector3 v)
        {
            Vector3 w = new Vector3();

            w.X = u.Y * v.Z - u.Z * v.Y;
            w.Y = u.Z * v.X - u.X * v.Z;
            w.Z = u.X * v.Y - u.Y * v.X;

            return w;
        }

        // A * X = B
        // A and B will be modified after execution
        // e.g.
        //    A00 * X0 + A01 * X1 = B0
        //    A01 * X0 + A11 * X1 = B1
        static public void SolveLinearEquations(double[,] A, double[] B, ref double[] X, int N)
        {
            // forward elimination
            for (int ii = 0; ii < N - 1; ii++)
            {
                // pivot row
                double pp = A[ii, ii];
                for (int kk = ii; kk < N; kk++)
                {
                    A[ii, kk] /= pp;
                }
                B[ii] /= pp;
                // eliminate unknown ii from rows below
                for (int jj = ii + 1; jj < N; jj++)
                {
                    double dd = A[jj, ii];
                    for (int kk = ii; kk < N; kk++)
                    {
                        A[jj, kk] -= A[ii, kk] * dd;
                    }
                    B[jj] -= B[ii] * dd;
                }
            }

            // backward substitution
            X[N - 1] = B[N - 1] / A[N - 1, N - 1];
            for (int ii = N - 2; ii >= 0; ii--)
            {
                X[ii] = B[ii];
                for (int jj = ii + 1; jj < N; jj++)
                {
                    X[ii] -= A[ii, jj] * X[jj];
                }
            }
        }

        static public bool Matrix_Inverse_2x2(double[,] src, ref double[,] dst)
        {
            if (src == null)
                throw new Exception("src cannot be null");

            double a = src[0,0];
            double b = src[0,1];
            double c = src[1,0];
            double d = src[1,1];
            double k = Matrix_Determinant_2x2(a,b,c,d);
            if (k == 0)
                return false;
            k = 1 / k;
            if (dst == null)
                dst = new double[2, 2];
            dst[0, 0] = k * d;
            dst[0, 1] = -k * b;
            dst[1, 0] = -k * c;
            dst[1, 1] = k * a;
            return true;
        }

        static public bool Matrix_Inverse_3x3(double[,] src, ref double[,] dst)
        {
            if (src == null)
                throw new Exception("src cannot be null");

            double determinant = Matrix_Determinant_3x3(src);
            if (Math.Abs(determinant) < 0.0000001)
                return false;
            double OneOverDeterminant = 1 / determinant;

            if (dst == null)
                dst = new double[3, 3];

            double a11 = src[0, 0];
            double a12 = src[0, 1];
            double a13 = src[0, 2];
            double a21 = src[1, 0];
            double a22 = src[1, 1];
            double a23 = src[1, 2];
            double a31 = src[2, 0];
            double a32 = src[2, 1];
            double a33 = src[2, 2];

            dst[0, 0] = OneOverDeterminant * Matrix_Determinant_2x2(a22, a23, a32, a33);
            dst[0, 1] = OneOverDeterminant * Matrix_Determinant_2x2(a13, a12, a33, a32);
            dst[0, 2] = OneOverDeterminant * Matrix_Determinant_2x2(a12, a13, a22, a23);

            dst[1, 0] = OneOverDeterminant * Matrix_Determinant_2x2(a23, a21, a33, a31);
            dst[1, 1] = OneOverDeterminant * Matrix_Determinant_2x2(a11, a13, a31, a33);
            dst[1, 2] = OneOverDeterminant * Matrix_Determinant_2x2(a13, a11, a23, a21);

            dst[2, 0] = OneOverDeterminant * Matrix_Determinant_2x2(a21, a22, a31, a32);
            dst[2, 1] = OneOverDeterminant * Matrix_Determinant_2x2(a12, a11, a32, a31);
            dst[2, 2] = OneOverDeterminant * Matrix_Determinant_2x2(a11, a12, a21, a22);

            return true;
        }

        static public double Matrix_Determinant_3x3(double[,] src)
        {
            double a = src[0, 0];
            double b = src[0, 1];
            double c = src[0, 2];
            double d = src[1, 0];
            double e = src[1, 1];
            double f = src[1, 2];
            double g = src[2, 0];
            double h = src[2, 1];
            double i = src[2, 2];

            return
                a * e * i +
                b * f * g +
                c * d * h -
                c * e * g -
                b * d * i -
                a * f * h;
        }

        static public double Matrix_Determinant_2x2(double m11, double m12, double m21, double m22)
        {
            return m11 * m22 - m12 * m21;
        }
    }

    static public class TextAssistant
    {
        static string[] separaters = new string[] { ", ", " ,", "," };
        static char[] trimCharactors = new char[] { ' ', '\n', '\t', '\r' };

        static public double[] StringToDoubleArray(string textString)
        {
            string[] entries = textString.Split(separaters, StringSplitOptions.RemoveEmptyEntries);
            double[] array = new double[entries.Length];
            for (int i = 0; i < array.Length; ++i)
            {
                string content = entries[i].Trim(trimCharactors);

                if (content.Contains("E+308"))
                {
                    if (content[0] == '-')
                        array[i] = double.MinValue;
                    else
                        array[i] = double.MaxValue;
                }
                else
                    array[i] = Double.Parse(content);
            }
            return array;
        }

        static public int[] StringToIntArray(string textString)
        {
            string[] entries = textString.Split(separaters, StringSplitOptions.RemoveEmptyEntries);
            int[] array = new int[entries.Length];
            for (int i = 0; i < array.Length; ++i)
            {
                string content = entries[i].Trim(trimCharactors);

                    array[i] = int.Parse(content);

            }
            return array;
        }

        static public string ArrayToString(double[] array)
        {
            string content = "";
            for (int i = 0; i < array.Length; ++i)
            {
                content += array[i];
                if (i < array.Length - 1)
                    content += ",";
            }
            return content;
        }

        static public string LoadFile(string fileName)
        {
            string rawFile = null;
            using (StreamReader sr = new StreamReader(fileName))
            {
                rawFile = sr.ReadToEnd();
            }
            return rawFile;
        }
    }

    public class BinaryMinHeap
    {
        private int Count;
        private double[] keyArray;
        private List<object> dataArray;

        public BinaryMinHeap(int size)
        {
            if (size < 0) throw new Exception("size of BinaryHeap cannot be negative");
            keyArray = new double[size];
            dataArray = new List<object>(size);
            for (int i = 0; i < size; ++i)
                dataArray.Add(new object());
            Reset();
        }

        public void Reset()
        {
            for (int i = 0; i < keyArray.Length; ++i)
            {
                keyArray[i] = double.MaxValue;
                dataArray[i] = null;
            }
            Count = 0;
        }

        public int GetCapacity()
        {
            return keyArray.Length;
        }

        public bool Pop(ref double key, ref object data)
        {
            if (Count <= 0) return false;

            key = keyArray[0];
            data = dataArray[0];

            keyArray[0] = keyArray[Count - 1];
            dataArray[0] = dataArray[Count - 1];

            keyArray[Count - 1] = double.MaxValue;
            dataArray[Count - 1] = null;

            Count--;

            int i = 0;
            while (i < Count - 1)
            {
                int childIdx = GetChildIndex(i, 0);
                if (childIdx > Count - 1) break;
                int childIdx_tmp = GetChildIndex(i, 1);
                if (childIdx_tmp < Count)
                    if (keyArray[childIdx] > keyArray[childIdx_tmp])
                        childIdx = childIdx_tmp;

                if (keyArray[i] <= keyArray[childIdx])
                    break;
                //swap
                double key_tmp = keyArray[childIdx];
                keyArray[childIdx] = keyArray[i];
                keyArray[i] = key_tmp;
                object data_tmp = dataArray[childIdx];
                dataArray[childIdx] = dataArray[i];
                dataArray[i] = data_tmp;
                //goto child
                i = childIdx;
            }
            return true;
        }

        public bool Push(double key, object data)
        {
            if (Count >= keyArray.Length)
                return false;
            //add to the bottom of the heap
            int i = Count;
            keyArray[i] = key;
            dataArray[i] = data;
            Count++;
            int j = GetParentIndex(i);
            while (j < i)
            {
                if (keyArray[j] <= keyArray[i])
                    break;
                //swap
                double key_tmp = keyArray[j];
                keyArray[j] = keyArray[i];
                keyArray[i] = key_tmp;
                object data_tmp = dataArray[j];
                dataArray[j] = dataArray[i];
                dataArray[i] = data_tmp;
                //goto parent
                i = j;
                j = GetParentIndex(i);
            }
            return true;
        }

        private int GetParentIndex(int index)
        {
            if (index == 0) return 0;
            if (index < 0) throw new Exception("index cannot be negative in binaryHeap");
            return (int)Math.Floor((index - 1) / 2.0);
        }

        private int GetChildIndex(int index, int isRightChild)
        {
            if (index < 0) throw new Exception("index cannot be negative in binaryHeap");
            if (isRightChild < 0 || isRightChild > 1) throw new Exception("isRightChild must be zero or one");

            return index * 2 + 1 + isRightChild;
        }
    }
}
